package star.core.util.geometry;

import javax.vecmath.Matrix3d;

/**
 * Classe che rappresenta la posizione 
 * nello spazio degli oggetti e delle 
 * rispettive gerarchie
 *
 */
public class POTransform  {

	private DoubleXYZ position;
	private Matrix3d orn;

	/**
	 * Costruttore della trasformata
	 * che contiene le informazioni di
	 * posizione e orientamento dell'oggetto
	 * nello spazio
	 * @param pos
	 */
	public POTransform(DoubleXYZ pos)
	{
		this.position=pos;
		this.orn=new Matrix3d();
	}


	public void setPosition(DoubleXYZ pos)
	{
		this.position=pos;
	}

	public DoubleXYZ getPosition()
	{
		return this.position;
	}

	public double dot(double[] v,int index)
	{
		double[] temp= new double[3];
		this.orn.getRow(index,temp);
		return temp[0]*v[0]+temp[1]*v[1]+temp[2]*v[2];
	}

	public double[] getRow(int index)
	{
		double[] temp= new double[3];
		this.orn.getRow(index,temp);
		return temp;
	}

	public void setInvMult(POTransform t1, POTransform t2) {
		double[] v=t2.position.setMinus(t1.position);
		this.position.setX(t1.dot(v,0));
		this.position.setY(t1.dot(v,1));
		this.position.setZ(t1.dot(v,2));
		this.orn = multT(t1,t2);
	}

	public Matrix3d getOrientation()
	{
		return this.orn;
	}

	
	public void setOrientation(Quaternion q)
	{
		double d = q.length2();
		assert(!eqz(d));
		double s = 2 / d;
		double xs = q.getX() * s,   ys = q.getY() * s,   zs = q.getZ() * s;
		double wx = q.getW() * xs,  wy = q.getW() * ys,  wz = q.getW() * zs;
		double xx = q.getX() * xs,  xy = q.getX() * ys,  xz = q.getX() * zs;
		double yy = q.getY() * ys,  yz = q.getY() * zs,  zz = q.getZ() * zs;

		this.orn.setM00(1 - (yy + zz));
		this.orn.setM01(xy + wz);
		this.orn.setM02( xz - wy);
		this.orn.setM10(xy - wz);
		this.orn.setM11(1 - (xx + zz));
		this.orn.setM12(yz + wx);
		this.orn.setM20(xz + wy);
		this.orn.setM21(yz - wx);
		this.orn.setM22(1 - (xx + yy));
	}


	public double[] getColumn(int index)
	{
		double[] temp= new double[3];
		this.orn.getColumn(index,temp);
		return temp;
	}


	public double[] operator(double[] v)
	{	
		double[] temp = new double[3];
		temp[0]=this.rowdot(0, v);
		temp[1]=this.rowdot(1, v);
		temp[2]=this.rowdot(2, v);
		return temp;
	}

	public  double rowdot(int i, double[] v) {
		return this.orn.getElement(0,i) * v[0] + this.orn.getElement(1,i) * v[1] + this.orn.getElement(2,i) * v[2];
	}

	public  double rowdot(int i, DoubleXYZ p) {

		return this.orn.getElement(0,i) * p.getX() + this.orn.getElement(1,i) * p.getY() + this.orn.getElement(2,i) * p.getZ();
	}

	public Matrix3d multT(POTransform t1, POTransform t2)
	{
		Matrix3d temp= new Matrix3d();
		temp.setM00(dot(t1.getRow(0),t2.getRow(0)));
		temp.setM01(dot(t1.getRow(1),t2.getRow(0)));
		temp.setM02(dot(t1.getRow(2),t2.getRow(0)));
		temp.setM10(dot(t1.getRow(0),t2.getRow(1)));
		temp.setM11(dot(t1.getRow(1),t2.getRow(1)));
		temp.setM12(dot(t1.getRow(2),t2.getRow(1)));
		temp.setM20(dot(t1.getRow(0),t2.getRow(2)));
		temp.setM21(dot(t1.getRow(1),t2.getRow(2)));
		temp.setM22(dot(t1.getRow(2),t2.getRow(2)));
		return temp;
	}
	
	private double dot(double[] v1,double[] v2)
	{
		return  v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
	}

	private boolean   eqz(double x) { return Math.abs(x) < 1e-10; }

	public Matrix3d absolute()
	{
		Matrix3d temp= new Matrix3d();
		temp.setM00(Math.abs(this.orn.getM00()));
		temp.setM01(Math.abs(this.orn.getM01()));
		temp.setM02(Math.abs(this.orn.getM02()));
		temp.setM10(Math.abs(this.orn.getM10()));
		temp.setM11(Math.abs(this.orn.getM11()));
		temp.setM12(Math.abs(this.orn.getM12()));
		temp.setM20(Math.abs(this.orn.getM20()));
		temp.setM21(Math.abs(this.orn.getM21()));
		temp.setM22(Math.abs(this.orn.getM22()));
		return temp;
	}

	public String toString() {
		return "pos: "+ this.position.getX()+"\t"+this.position.getY()+"\t"+this.position.getZ() + "\norn: "+ orn.m00 +"\t"+orn.m01 +"\t"+orn.m02 + 
		orn.m10 +"\t"+orn.m11 +"\t"+orn.m12 + 
		orn.m20 +"\t"+orn.m21 +"\t"+orn.m22; 
	}
}
